%%  Skrypt dla estymacji kolejnego stanu za pomoca filtru kalmana
% Parametry:
%   system.x    -   wektor stanu "a priori" (jesli puste - zostanie
%                   wyznaczone)
%   system.y    -   wyjscie
%   system.u    -   sterowanie
%   system.A    -   macierz stanu
%   system.B    -   macierz wejscia
%   system.C    -   macierz wyjscia
%   system.P    -   kowariancja wektora stanu, na wejsciu "a priori" na
%                   wyjsciu "a posteriori"
%   system.Q    -   kowariancja wejscia
%   system.R    -   kowariancja wyjscia
%
% System liniowy:
%   x'  =   Ax + Bu + w
%   y   =   Cx + v
% gdzie:
%   w   -   szum gausowski o kowariancji Q
%   v   -   szum gausowski o kowariancji R

function system = kalmanuj(system)

if isnan(system.x)

    %% inicjalizacja stanu na podstawie pierwszej obserwacji
    system.x = inv(system.C)*system.y;
	system.P = inv(system.C)*system.R*inv(system.C');

else

    %% predykcja wektora stanu i kowariancji
    system.x = system.A*system.x + system.B*system.u;
    system.P = system.A * system.P * system.A' + system.Q;

	%% wzmocnienie kalmana
	K = system.P*system.C'*inv(system.C*system.P*system.C'+system.R);

	%% korekcja na podstawie obserwacji
	system.x = system.x + K*(system.y-system.C*system.x);
	system.P = system.P - K*system.C*system.P;

end